#include <stdlib.h>

#include "dsp.h"

#include "../matrix.h"
#include "../../util/macros.h"
#include "../../util/misc.h"

void matrix_conv(Matrix * Mat, Matrix * Mask, Matrix ** Conv) {

	double * mat;
	double * conv = NULL;

	int r, c, _r, i;
	int mr, mc, _mr, mi;

	int rows;
	int columns;
	long int matsize;

	int d;

	double sum;

	/* ====================================================================== */
	{
		_verify(Mat);
		_verify(Conv);
	}
	/* ====================================================================== */
	{
		mat = Mat->data;
		rows = Mat->rows;
		columns = Mat->columns;
		matsize = Mat->size;
	}
	/* ====================================================================== */
	{
		if (*Conv == NULL) (*Conv) = matrix_new(rows, columns);
		conv = (*Conv)->data;
	}

	/* Convolve with Mask =================================================== */
	{
		double * mask = Mask->data;

		int mrows = Mask->rows;
		int mcolumns = Mask->columns;
		int msize_minus_1 = Mask->size - 1;

		int rf, cf;

		int mrc = (mrows - 1) / 2;
		int mcc = (mcolumns - 1) / 2;		

		int _2columns_minus_2 = (columns << 1) - 2;
		int _2row_minus_2 = (rows << 1) - 2;

		int * r_shift = create_int_sequence(rows, 0, columns);

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {

				i = _r + c;
				sum = 0;

				for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {

					// reflect y boundaries
					rf = r + (mr - mrc);
					if (rf < 0) rf = -rf;
					else if (rf >= rows) rf = _2row_minus_2 - rf;

					for (mc = 0; mc < mcolumns; mc++) {

						// reflect x boundaries
						cf = c + (mc - mcc);
						if (cf < 0) cf = -cf;
						else if (cf >= columns) cf = _2columns_minus_2 - cf;

						//d = rf * columns + cf;
						d = r_shift[rf] + cf;

						// reflect the mask index
						mi = _mr + mc;
						mi = msize_minus_1 - mi;

						sum += mat[d] * mask[mi];
					}
				}
				conv[i] = sum;
			}
		}// </editor-fold>

		free(r_shift);
	}
}

void matrix_conv_2_pass(Matrix * Mat, Matrix * Mask1, Matrix * Mask2, Matrix ** Conv) {

	double * mat = NULL;
	double * conv = NULL;

	Matrix * _Conv = NULL;
	double * _conv = NULL;

	double * mask = NULL;

	int r, c, _r, i;
	int mr, mc, _mr, mi;

	int rows;
	int columns;
	long int matsize;

	int mrows;
	int mcolumns;

	int d;

	double sum;

	/* ====================================================================== */
	{
		_verify(Mat);
		_verify(Conv);
	}
	/* ====================================================================== */
	{
		mat = Mat->data;
		rows = Mat->rows;
		columns = Mat->columns;
		matsize = Mat->size;
	}
	/* ====================================================================== */
	{
		if (*Conv == NULL) (*Conv) = matrix_new(rows, columns);
		conv = (*Conv)->data;

		_Conv = matrix_new(rows, columns);
		_conv = _Conv->data;
	}

	/* Step 01: Convolve with Mask1 ========================================= */
	{
		mask = Mask1->data;

		mrows = Mask1->rows;
		mcolumns = Mask1->columns;

		int rf;
		int mrc = (mrows - 1) / 2;
		int _2row_minus_2 = (rows << 1) - 2;

		int * r_shift = create_int_sequence(rows, 0, columns);

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {

				i = _r + c;
				sum = 0;

				for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {

					// reflect y boundaries
					rf = r + (mr - mrc);
					if (rf < 0) rf = -rf;
					else if (rf >= rows) rf = _2row_minus_2 - rf;

					//d = rf * columns + c;
					d = r_shift[rf] + c;

					mi = (mrows - mr - 1);

					sum += mat[d] * mask[mi];

				}
				_conv[i] = sum;
			}
		}// </editor-fold>

		free(r_shift);
	}

	/* Step 02: Convolve with MaskRow ======================================= */
	{
		mask = Mask2->data;

		mrows = Mask2->rows;
		mcolumns = Mask2->columns;

		int cf;
		int mcc = (mcolumns - 1) / 2;
		int _2columns_minus_2 = (columns << 1) - 2;

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {

				i = _r + c;
				sum = 0;

				for (mc = 0; mc < mcolumns; mc++) {

					cf = c + (mc - mcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					d = _r + cf;

					mi = _mr + mc;
					mi = (mcolumns - mc - 1);

					sum += _conv[d] * mask[mi];
				}
				conv[i] = sum;
			}
		}// </editor-fold>

	}

	matrix_delete(_Conv);

}

/*
 * =============================================================================
 * =============================================================================
 */

/*
Matrix * __matrix_create_conv_index_shift(int mask_rows, int mask_columns, int data_columns) {
	Matrix * Shift;
	int r, c;

	double * shift;

	int cr, cc;

	cr = (mask_rows - 1) / 2;
	cc = (mask_columns - 1) / 2;

	Shift = matrix_new(mask_rows, mask_columns);
	shift = Shift->data;

	for (r = 0; r < mask_rows; r++) {
		for (c = 0; c < mask_columns; c++) {
			//shift[r * mask_columns + c] = (r - cr) * data_columns + (c - cc);
			shift[r * mask_columns + c] = (cr - r) * data_columns + (cc - c);
		}
	}
	return Shift;
}

void matrix_conv_zb(const Matrix * Mat, const Matrix * Mask, Matrix ** Conv) {

	double * mat; // Alias to Mat data pointer 
	double * mask; // Alias to Mask data pointer 
	double * conv; // Alias to Conv data pointer 

	Matrix * Shift; // Matrix of index shift 
	double * shift; // Alias to Shift data pointer 

	int r, c, _r, i; // Mat indexes 
	int mr, mc, _mr, mi; // Mask indexes 

	int rows; // Mat rows 
	int columns; // Mat columns 

	int mrows; // Mask rows 
	int mcolumns; // Mask columns 

	long int matsize; // Size of Mat 
	int d; // Shifted index 
	double sum; // Convolution sum 

	_verify(Mat);
	_verify(Mask);
	_verify(Conv);
	if (*Conv == NULL) (*Conv) = matrix_new(Mat->rows, Mat->columns);

	conv = (*Conv)->data;

	mat = Mat->data;
	rows = Mat->rows;
	columns = Mat->columns;
	matsize = Mat->size;

	mask = Mask->data;
	mrows = Mask->rows;
	mcolumns = Mask->columns;

	Shift = matrix_create_conv_index_shift(mrows, mcolumns, columns);
	shift = Shift->data;

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sum = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= matsize) continue;

					sum += mat[d] * mask[mi];
				}
			}
			conv[i] = sum;
		}
	}
	matrix_delete(Shift);
}

void matrix_conv_2_pass_zb(const Matrix * Mat, const Matrix * Mask1, const Matrix * Mask2, Matrix ** Conv) {

	Matrix * Shift; // Matrix of index shift 
	double * shift; // Alias to Shift data pointer 

	Matrix * _Conv;
	double * _conv;

	double * mat; // Alias to Mat data pointer 
	double * mask; // Alias to Mask data pointer 
	double * conv; // Alias to Conv data pointer 

	int r, c, _r, i; // Mat indexes 
	int mr, mc, _mr, mi; // Mask indexes 

	int rows; // Mat rows 
	int columns; // Mat columns 

	int mrows; // Mask rows 
	int mcolumns; // Mask columns 

	long int matsize; // Size of Mat 
	int d; // Shifted index 
	double sum; // Convolution sum 

	_verify(Mat);
	_verify(Mask1);
	_verify(Mask2);
	_verify(Conv);

	_Conv = matrix_new(Mat->rows, Mat->columns);
	_conv = _Conv->data;

	if (*Conv == NULL) (*Conv) = matrix_new(Mat->rows, Mat->columns);

	conv = (*Conv)->data;

	mat = Mat->data;
	rows = Mat->rows;
	columns = Mat->columns;
	matsize = Mat->size;

	// Step 01: Setup mask1 
	mask = Mask1->data;
	mrows = Mask1->rows;
	mcolumns = Mask1->columns;

	Shift = matrix_create_conv_index_shift(Mask1->rows, Mask1->columns, columns);
	shift = Shift->data;

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sum = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= matsize) continue;

					sum += mat[d] * mask[mi];
				}
			}
			_conv[i] = sum;
		}
	}
	matrix_delete(Shift);

	// Step 02: Setup mask2 
	mask = Mask2->data;
	mrows = Mask2->rows;
	mcolumns = Mask2->columns;

	Shift = matrix_create_conv_index_shift(Mask2->rows, Mask2->columns, columns);
	shift = Shift->data;

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;
			sum = 0;

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;
					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= matsize) continue;

					sum += _conv[d] * mask[mi];
				}
			}
			conv[i] = sum;
		}
	}

	matrix_delete(Shift);
	matrix_delete(_Conv);
}
 */
